#include "canbus/message.h"

/**
 * @namespace canbus
 * @brief canbus
 */
namespace canbus {

/**
 * @class CanMsg11
 * @brief Concrete can message that stores vehicle state information
 */
class CanMsg11: public CanMsg {
public:
  CanMsg11():
  CanMsg(),
  gantry_speed(0),
  gantry_angle(0),
  gantry_radius(0),
  gantry_mode(1)  
  {}

  CanMsg11(uint8_t* value): 
  CanMsg(value),
  gantry_speed(0),
  gantry_angle(0),
  gantry_radius(0),
  gantry_mode(1) 
  {}

  virtual ~CanMsg11() = default;

  void UpUpdateData() {
    gantry_speed =  (ParseTwins(*(data_+1), *(data_+0)) - speed_offset)/speed_unit;
    gantry_angle =  (ParseTwins(*(data_+3), *(data_+2)) - angle_offset)/angle_unit;
    gantry_radius = (ParseTwins(*(data_+5), *(data_+4)) - radius_offset)/radius_unit;
    gantry_mode =    ParseTwins(*(data_+7), *(data_+6));
  }

  template <typename T>
  void DownUpdateData(const T& input_data) {}

  static const int32_t ID;
  static const std::string NAME;

  double gantry_speed;
  double gantry_angle;
  double gantry_radius;
  double gantry_mode;

  const int speed_offset = 1250;
  const int speed_unit = 10; 
  const int angle_offset = 5000;
  const int angle_unit = 100;
  const int radius_offset = 10000;
  const int radius_unit = 100; 

  friend class CanbusHandler;
};

}